/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client commanding joint group with swig wrapped c++ library in python
import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")
from PyQt4 import QtCore, QtGui
import JointGroupCtrlClient
import gimi
#if sys.argv.__len__() != 3:
#	print "Usage: python %s clientName serverName"%sys.argv[0]
#	sys.exit()



class JDialog (QtGui.QDialog):
	def __init__(self, jgcc):
		
		#Set class variables
		self.joints = []
		self.jgcc = jgcc
		self.jointCount = jgcc.GetJointNumber()
		if self.jointCount == 0:
			print "No joints found"
			return None
		#Setup empty Dialog
		QtGui.QDialog.__init__(self)
		self.setObjectName("JointDialog")
		self.resize(175*self.jointCount, 305)
		sizePolicy = QtGui.QSizePolicy(QtGui.QSizePolicy.Fixed, QtGui.QSizePolicy.Fixed)
		sizePolicy.setHorizontalStretch(0)
		sizePolicy.setVerticalStretch(0)
		sizePolicy.setHeightForWidth(self.sizePolicy().hasHeightForWidth())
		self.setSizePolicy(sizePolicy)
		# Set up the user interface for each joint
		for i in range(self.jointCount):
			info = JointGroupCtrlClient.TJointInfo("huhuu",5, JointGroupCtrlClient.KJointControlAngular)
			ret = jgcc.GetJointInformation(info, i, 1000)
			self.joints.append(joint(self, info, i))
		self.pollTimer = QtCore.QTimer(self)
		self.connect(self.pollTimer,QtCore.SIGNAL("timeout()"),self.poll)
		self.pollTimer.start(1000)#1000 ms interval for polling
	
	def poll(self):
		"""Poll joint poses"""
		self.pollTimer.stop()
		for j in self.joints:
			pos = 9.9
			ret = self.jgcc.GetPosition(j.count, pos, 1000)
			if ret[0]:
				j.setCurPos(ret[1])
		self.pollTimer.start(1000)#1000 ms interval for polling



class joint:
	def __init__(self, JointDialog, jointInfo, count):
		self.minPos = jointInfo.minPosition
		self.maxPos = jointInfo.maxPosition
		self.info = jointInfo
		self.width = 175
		self.count = count
		self.parent = JointDialog
		self.jointRange = self.info.maxPosition - self.info.minPosition
		if self.info.maxPosition == self.info.minPosition:
			self.jointRange = 0.00001
		
		#draw elements
		self.verticalSliderCom = QtGui.QSlider(JointDialog)
		self.verticalSliderCom.setGeometry(QtCore.QRect(30 + count*self.width, 60, 25, 160))
		self.verticalSliderCom.setOrientation(QtCore.Qt.Vertical)
		self.verticalSliderCom.setObjectName("verticalSliderCom")
		self.verticalSliderCom.setMaximum(100)
		self.verticalSliderPos = QtGui.QSlider(JointDialog)
		self.verticalSliderPos.setEnabled(False)
		self.verticalSliderPos.setGeometry(QtCore.QRect(60 + count*self.width, 60, 25, 160))
		self.verticalSliderPos.setOrientation(QtCore.Qt.Vertical)
		self.verticalSliderPos.setObjectName("verticalSliderPos")
		self.verticalSliderPos.setMaximum(100)
		self.labelName = QtGui.QLabel(JointDialog)
		self.labelName.setGeometry(QtCore.QRect(30 + count*self.width, 10, 121, 20))
		font = QtGui.QFont()
		font.setPointSize(12)
		font.setWeight(75)
		font.setBold(True)
		self.labelName.setFont(font)
		self.labelName.setObjectName("labelName")
		self.labelMax = QtGui.QLabel(JointDialog)
		self.labelMax.setGeometry(QtCore.QRect(90 + count*self.width, 60, 59, 20))
		self.labelMax.setObjectName("labelMax")
		self.labelMin = QtGui.QLabel(JointDialog)
		self.labelMin.setGeometry(QtCore.QRect(90 + count*self.width, 200, 59, 20))
		self.labelMin.setObjectName("labelMin")
		self.lineEdit = QtGui.QLineEdit(JointDialog)
		self.lineEdit.setGeometry(QtCore.QRect(20 + count*self.width, 220, 81, 29))
		self.lineEdit.setObjectName("lineEdit")
		self.goButton = QtGui.QPushButton(JointDialog)
		self.goButton.setGeometry(QtCore.QRect(100 + count*self.width, 220, 31, 28))
		self.goButton.setObjectName("goButton")
		self.labelPos = QtGui.QLabel(JointDialog)
		self.labelPos.setGeometry(QtCore.QRect(90 + count*self.width, 130, 59, 20))
		self.labelPos.setObjectName("labelPos")
		self.labelType = QtGui.QLabel(JointDialog)
		self.labelType.setGeometry(QtCore.QRect(30 + count*self.width, 40, 59, 20))
		self.labelType.setObjectName("labelType")
		self.labelSpeedInfo = QtGui.QLabel(JointDialog)
		self.labelSpeedInfo.setGeometry(QtCore.QRect(30 + count*self.width, 260, 121, 20))
		self.labelSpeedInfo.setObjectName("labelSpeedInfo")

		self.retranslateUi(JointDialog)
		QtCore.QObject.connect(self.verticalSliderCom, QtCore.SIGNAL("valueChanged(int)"), self.moveJointInt)
		QtCore.QObject.connect(self.goButton, QtCore.SIGNAL("clicked()"), self.moveJointButton)
		QtCore.QMetaObject.connectSlotsByName(JointDialog)
		print "greated %s joint"%self.info.jointName

	def retranslateUi(self, JointDialog):
		JointDialog.setWindowTitle(QtGui.QApplication.translate("JointDialog", "Dialog", None, QtGui.QApplication.UnicodeUTF8))
		self.goButton.setText(QtGui.QApplication.translate("JointDialog", "Go", None, QtGui.QApplication.UnicodeUTF8))
		self.labelName.setText(QtGui.QApplication.translate("JointDialog", self.info.jointName, None, QtGui.QApplication.UnicodeUTF8))
		self.labelMax.setText(QtGui.QApplication.translate("JointDialog", str(self.info.maxPosition), None, QtGui.QApplication.UnicodeUTF8))
		self.labelMin.setText(QtGui.QApplication.translate("JointDialog", str(self.info.minPosition), None, QtGui.QApplication.UnicodeUTF8))
		self.labelPos.setText(QtGui.QApplication.translate("JointDialog", "unknown", None, QtGui.QApplication.UnicodeUTF8))
		if self.info.jointControlType == JointGroupCtrlClient.KJointControlAngular:
			self.labelType.setText(QtGui.QApplication.translate("JointDialog", "Revolving", None, QtGui.QApplication.UnicodeUTF8))
		elif self.info.jointControlType == JointGroupCtrlClient.KJointControlLinear:
			self.labelType.setText(QtGui.QApplication.translate("JointDialog", "Linear", None, QtGui.QApplication.UnicodeUTF8))
		if self.info.canControlSpeed:
			self.labelSpeedInfo.setText(QtGui.QApplication.translate("JointDialog", "Speed controlable", None, QtGui.QApplication.UnicodeUTF8))
		else:
			self.labelSpeedInfo.setText(QtGui.QApplication.translate("JointDialog", "No speed control", None, QtGui.QApplication.UnicodeUTF8))

	def setCurPos(self, pos):
		#print "set pose", pos
		self.labelPos.setText(QtGui.QApplication.translate("JointDialog", str(pos), None, QtGui.QApplication.UnicodeUTF8))
		self.verticalSliderPos.setValue(int(100*(pos - self.info.minPosition)/self.jointRange))
		
	def moveJointInt(self, command):
		print "int command ", command,
		com = self.info.minPosition + self.jointRange * float(command) / 100.
		print "float command ", com
		
	def moveJoint(self, command):
		if command > self.info.maxPosition: command = self.info.maxPosition
		if command < self.info.minPosition: command = self.info.minPosition
		ret = jgcc.SetPosition(command,self.count)
		if not ret:
			print ret, "moveJoint failed"

	def moveJointButton(self):
		line = str(self.lineEdit.text())
		self.moveJoint(float(line))

if __name__ == "__main__":
	g = gimi.GIMI()
	jgcc = JointGroupCtrlClient.CJointGroupCtrlClient(g, 0)
	if sys.argv.__len__() != 4:
		print "Usage: python %s hub port jointServerName"%sys.argv[0]
		sys.exit()
	
	
	#ret = g.connectToHubEx("asrobo.hut.fi", 50002, sys.argv[1])
	print "connecting to hub %s:%i ..."%(sys.argv[1], int(sys.argv[2])),
	ret = g.connectToHubEx(sys.argv[1], int(sys.argv[2]), "", 1000)
	print JointGroupCtrlClient.GetErrorStr(ret)
	if ret != 0:
		print "Exiting..."
		sys.exit()
	print "set target... ",
	if sys.argv[3].find(".") < 0:
		print sys.argv[3]
		jgcc.SetDefaultTarget(sys.argv[3])
	else:
		print "with fancy name ", sys.argv[3]
		jgcc.SetDefaultTarget(JointGroupCtrlClient.SMaCISL(sys.argv[3]))
	
	print "\nopen...",
	ret = jgcc.Open()
	print JointGroupCtrlClient.GetErrorStr(ret)
	if ret != 0:
		print "Exiting..."
		sys.exit()
	del ret
	
	app = QtGui.QApplication(sys.argv)
	f = JDialog(jgcc)
	if f:
		print "show"
		f.show()
		sys.exit(app.exec_())
	else:
		print "nothing to show"
		jgcc.Close()
		sys.exit()
	
	print "Main finished too"
